#!/usr/bin/env python

# Copyright FYT 邹承甫 2022

import roslib
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image
import cv2
import numpy as np
from cv_bridge import CvBridge

def main():
    cnt = 0
    rospy.init_node("video_plyaer", anonymous=True)

    path = rospy.get_param("/camera/video_path")
    start_cnt = rospy.get_param("/camera/video_start_frame")
    pub = rospy.Publisher("raw_img", Image, queue_size=10)
    bridge = CvBridge()
    rate = rospy.Rate(60)
    cap = cv2.VideoCapture(path)

    if cap.isOpened() == False:
        print("Error in opening video stream or file\n视频打开失败，没有这个路径")
        return
    else:
        print("Video has been opened, start frame is {}".format(start_cnt[0]*(start_cnt[1]*60 + start_cnt[2])))
    while(cap.isOpened())and(not rospy.is_shutdown()):
        ret, frame = cap.read()
        if ret:
            if cnt < start_cnt[0]*(start_cnt[1]*60 + start_cnt[2]):
                cnt+=1
                continue
            # cv2.imshow('Frame',frame)
            msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")
            header = Header()
            header.stamp = rospy.Time.now()
            header.frame_id = "camera"
            msg.header = header
            pub.publish(msg)
            rate.sleep()
            if cv2.waitKey(1) & 0xFF == 27:
                break
        else:
            break

    cap.release()
    cv2.destroyAllWindows()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

